#include "lasercameraadapter.h"
#include "modbusmaster.h"
#include "domparser.h"
#include "messagelog.h"
#include "mccontrollerthread.h"

#define D_DEBUG_FLAG 0
#define D_TIME_PERIOD 50
#define D_WRITE_REGISTER_TRY_COUNT 150
#define D_TRY_WAIT 10000

LaserCameraAdapter::LaserCameraAdapter(QString configFileIn, int robotIdIn,
                         McControllerThread *programMcControllerIn, QObject *parent) : QObject(parent)
{
    programMcController=programMcControllerIn;
    isInitialOkStatus=false;
    robotId=robotIdIn;
    configFilePath=configFileIn;
    modbusMaster=NULL;
    cameraStatus.connectStatus=0;
    isReadRegister=true;
    if(1==initialConfigFile(configFileIn))
    {
        if(true==isUseThisModule)
        {
            timer=new QTimer();

            connectLaserCamera_slot();
            if(1==cameraStatus.connectStatus)
            {
                   timer->start( D_TIME_PERIOD );
                   isInitialOkStatus=true;
            }
            else
            {
                addMsg( ENUM_MSG_ERROR,  "LaserCameraAdapter", "LaserCameraAdapter",  12552,  robotId);
            }
            connect( timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()) );
            connect(this, SIGNAL(writeRegister_signal(int,int)), this,SLOT(writeRegister_slot(int,int)));
            connect(this, SIGNAL(connectLaserCamera_signal()), this,SLOT(connectLaserCamera_slot()));
            connect(modbusMaster, SIGNAL(readSucess_signal()), this,SLOT(readSucess_slot()),Qt::DirectConnection);
        }
        else
        {
            isInitialOkStatus=true;
        }


    }
    else
    {
        addMsg( ENUM_MSG_ERROR,  "LaserCameraAdapter", "LaserCameraAdapter",  12551,  robotId);
    }
}

int LaserCameraAdapter::setIsReadRegister(bool isRead)
{
    isReadRegister=isRead;
    return 1;
}

int LaserCameraAdapter::getLaserCameraConfigParameter(LaserCameraConfigParameter &infoOut)
{
    infoOut=cameraConfigParameter;
    return 1;
}

int LaserCameraAdapter::getLaserCameraStatus(LaserCameraStatusInfo &infoOut)
{
    infoOut=cameraStatus;
    return 1;
}

int LaserCameraAdapter::setLaserCameraConfigParameter(LaserCameraConfigParameter parameter)
{
    cameraConfigParameter=parameter;
    writeConfigFile(configFilePath,parameter);
    if(1==reconnectLaserCamera())
    {
    }
    else
    {
        addMsg( ENUM_MSG_ERROR,  "LaserCameraAdapter", "LaserCameraAdapter",  12552,  robotId);
    }
    return 1;
}

int LaserCameraAdapter::reconnectLaserCamera()
{
    emit connectLaserCamera_signal();
    return 1;
}

int LaserCameraAdapter::setLaserCameraWork(bool isWork)
{

    int tmpAddress;
    int tmpCommand;
    switch (cameraConfigParameter.protocol) {
    case 0:
        tmpAddress=2;
        if(isWork)
        {
           tmpCommand=255;
        }
        else
        {
            tmpCommand=0;
        }

        break;
    default:
        break;
    }
    emit writeRegister_signal(tmpAddress,tmpCommand);
    return 1;


}

int LaserCameraAdapter::setLaserCameraShowLaser(bool isOn)
{

    int tmpAddress;
    int tmpCommand;
    switch (cameraConfigParameter.protocol) {
    case 0:
        tmpAddress=1;
        if(isOn)
        {
           tmpCommand=255;
        }
        else
        {
            tmpCommand=0;
        }

        break;
    default:
        break;
    }
    emit writeRegister_signal(tmpAddress,tmpCommand);
    return 1;
}

int LaserCameraAdapter::setLaserCameraJobId(int jobIdIn)
{

    int tmpAddress;
    int tmpCommand;
    switch (cameraConfigParameter.protocol) {
    case 0:
        tmpAddress=0;
        tmpCommand=jobIdIn;
        break;
    default:
        break;
    }

    emit writeRegister_signal(tmpAddress,tmpCommand);
    return 1;

}
#define NSEC_PER_SEC    1000000000
void LaserCameraAdapter::realTimeSleep(long long umicoroSecond)
{
    // 处理qt事件
    QCoreApplication::processEvents();

    struct timespec time;
    clock_gettime(CLOCK_REALTIME,&time);
    time.tv_nsec += umicoroSecond * 1000;
    while(time.tv_nsec >= NSEC_PER_SEC)
    {
        time.tv_nsec -= NSEC_PER_SEC;
        ++time.tv_sec;
    }
    clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&time,NULL);

}


void LaserCameraAdapter::timeOut_slot()
{
//    return ;
    if(NULL==modbusMaster || 1!=cameraStatus.connectStatus)
    {
        return ;
    }

    if(2!=modbusMaster->getConnectState())
    {
        cameraStatus.connectStatus=-2;
        return ;
    }

    if(D_DEBUG_FLAG)
    {
        qDebug()<<"timeout send read";
    }

    //读取寄存器(发命令)
    if(1==cameraStatus.connectStatus && isReadRegister)
    {
        QModbusDataUnit::RegisterType type=QModbusDataUnit::HoldingRegisters;
        int newStartAddress=0;
        QVector<quint16> newValues;
        newValues.resize(15);
        QModbusDataUnit modbusDateUnit(type,newStartAddress,newValues);
        if(READ_WRITE_RESULT_STATE_WAITING==modbusMaster->readModbusDataUnit(modbusDateUnit,1))
        {

        }
        else
        {

            return ;
        }
    }


}

void LaserCameraAdapter::readSucess_slot()
{
    //取读的结果
    QModbusDataUnit modbusDateUnitForResult;
    modbusMaster->getReadModbusDataUnit(modbusDateUnitForResult);
        //    qDebug()<<"read success";
        cameraStatus.regReadSucessCount++;
        int count=modbusDateUnitForResult.valueCount();
//        for(int i=0;i<count;i++)
//        {
//            qDebug()<<"modbusDateUnit.value(i)"<<i<<"="<<modbusDateUnitForResult.value(i);
//        }
        cameraStatus.connectStatus=1;
        switch (cameraConfigParameter.protocol) {
        case 0:

            cameraStatus.isFind=modbusDateUnitForResult.value(9);
            cameraStatus.jobId=modbusDateUnitForResult.value(0);
            cameraStatus.laserStatus=modbusDateUnitForResult.value(2);
            cameraStatus.origin_x=(short)modbusDateUnitForResult.value(10);
            cameraStatus.origin_z=(short)modbusDateUnitForResult.value(11);
            if(D_DEBUG_FLAG)
            {
                qDebug()<<"read received: cameraStatus.regReadSucessCount"<<cameraStatus.regReadSucessCount;
            }


            break;
        default:
            break;
        }

        if(255==cameraStatus.isFind)
        {
            cameraStatus.real_x=cameraStatus.origin_x/cameraConfigParameter.xRatio;
            cameraStatus.real_z=cameraStatus.origin_z/cameraConfigParameter.zRatio;
            programMcController->mcControllerInterpreter->setCameraPointCoordinate(cameraStatus.real_x,cameraStatus.real_z,0);

        }


}

void LaserCameraAdapter::writeRegister_slot(int address, int value)
{
    setIsReadRegister(false);
    timer->stop();
    QModbusDataUnit::RegisterType type=QModbusDataUnit::HoldingRegisters;
    QVector<quint16> newValues;
    newValues.resize(1);
    newValues[0]=value;
    QModbusDataUnit modbusDateUnit(type,address,newValues);
    int tryCount=0;
    int writeResult;
    do
    {
        writeResult=modbusMaster->writeModbusDataUnit(modbusDateUnit,1);
        if(1!=writeResult)
        {
            realTimeSleep(D_TRY_WAIT);//确保等待时，可以处理modbus收到数据信号
//            usleep(D_TRY_WAIT);
            tryCount++;
            qDebug()<<"writeResult"<<writeResult;
        }
        else
        {
            break;
        }

    }while(tryCount<D_WRITE_REGISTER_TRY_COUNT);
    qDebug()<<"write result"<<writeResult<<"address"<<address<<"value"<<value;
    if(1!=writeResult)
    {
         addMsg( ENUM_MSG_ERROR,  "LaserCameraAdapter", "LaserCameraAdapter",  12553,  robotId,address);
    }

    setIsReadRegister(true);
    timer->start();
}

void LaserCameraAdapter:: connectLaserCamera_slot()
{
    if(NULL!=modbusMaster)
    {
        modbusMaster->disConnectPort();
        delete modbusMaster;
    }
    ModbusConnection typeIn=Tcp;
    QString portNameIn="";
    SerialSettings serialSettingIn;
//    QString ip_port_in="127.0.0.1:567";
    QString ip_port_in=QString::fromStdString(cameraConfigParameter.ip);
    ip_port_in+=":";
    ip_port_in+=QString::number(cameraConfigParameter.port);
//    QString ip_port_in="10.1.21.15:567";
    modbusMaster = new ModbusMaster(typeIn,portNameIn,serialSettingIn,ip_port_in);

    connect(modbusMaster, SIGNAL(readSucess_signal()), this,SLOT(readSucess_slot()),Qt::DirectConnection);
    //connect port
    if(1==modbusMaster->connectPort())
    {
        qDebug()<<"connect success";
        //需要等待,否则可能会通讯不上
//        sleep(1);
        cameraStatus.connectStatus=1;
        return ;

    }
    else
    {
        qDebug()<<"connect failed";
        cameraStatus.connectStatus=-1;
        return ;
    }
    return ;
}

int LaserCameraAdapter::writeConfigFile(QString configFileName, LaserCameraConfigParameter parameterIn)
{
    qDebug() << "LaserCameraAdapter::writeConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadWrite);
    if( !ok )
    {

        qDebug() << " 配置文件打开失败：" << configFileName;
        return -1;
    }


    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "protocol", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.writeXml(domNode, parameterIn.protocol);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "ip", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.writeXml(domNode, QString::fromStdString(parameterIn.ip));

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "port", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.writeXml(domNode, parameterIn.port);
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "xRatio", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.writeXml(domNode, parameterIn.xRatio);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "zRatio", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.writeXml(domNode, parameterIn.zRatio);


    domparser.closeXml(true);
}

int LaserCameraAdapter::initialConfigFile(QString configFileName)
{
    qDebug() << "LaserCameraAdapter::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {

        qDebug() << " 配置文件打开失败：" << configFileName;
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseThisModule", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, isUseThisModule);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "protocol", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, cameraConfigParameter.protocol);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "ip", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, cameraConfigParameter.ip);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "port", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, cameraConfigParameter.port);
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "xRatio", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, cameraConfigParameter.xRatio);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "zRatio", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, cameraConfigParameter.zRatio);


    domparser.closeXml();
    return 1;
}

void LaserCameraAdapter::addMsg(int messageLevel, std::string componentName, std::string messageType, int messageCode, int robotId, int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;


    QString infomationStr;

    switch(messageCode)
    {
    case 12553:
    {
        infomationStr =  QObject::tr("激光相机写寄存器%1失败！").arg(parameter1);
        break;
    }
    case 12552:
    {
        infomationStr =  QObject::tr("激光相机连接失败！");
        break;
    }
    case 12551:
    {
        infomationStr =  QObject::tr("LaserCameraAdapter配置文件加载失败！");
        break;
    }

    default:

        break;
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    MessageLog::getInstance()->addMessage(tmpMsg);
}
